﻿#include "SPP.h"
#include <iostream>
#include <cmath>
#include <vector>
#include "Matrix.h"
using namespace std;

SPP::SPP(GNSSconsts* gnssConsts, TimeSys* timeSys, CoordSys* coordSys, ReadN* readN, ReadO* readO)
    : mGnssConsts(gnssConsts), mTimeSys(timeSys), mCoordSys(coordSys), mReadN(readN), mReadO(readO) {
    mSatPos = new BDSSatPos(timeSys, readN);
    mIonoCorr = new IonoCorr(gnssConsts, &readN->get_head());
    mTropCorr = new TropCorr(gnssConsts);
}

XYZ SPP::calculate_ReceiverPosition() {
    vector<obsEpoch>* obs = mReadO->get_obs();
    if (obs->empty()) {
        cerr << "No observation data." << endl;
        return { 0, 0, 0 };
    }

    obsEpoch firstEpoch = obs->at(0);
    BaseT obsTime = mTimeSys->str2baseT(firstEpoch.time);
    SOW sow = mTimeSys->calend2sow({ 2017, 3, 7, 5, 6, 35, 0.00 });

    // 初始接收机位置，这里简单假设一个值
    XYZ recPos = { 0, 0, 0 };
    double dt = 0; // 接收机钟差

    for (int iter = 0; iter < 10; ++iter) { // 迭代求解
        vector<XYZ> satPositions = get_SatPositions(firstEpoch, obsTime);
        vector<double> pseudoranges = get_Pseudoranges(firstEpoch, satPositions, recPos, sow);

        leastSquares(satPositions, pseudoranges, recPos, dt);
    }

    return recPos;
}

vector<XYZ> SPP::get_SatPositions(const obsEpoch& epoch, BaseT obsTime) {
    vector<XYZ> satPositions;
    for (const auto& sat : epoch.listObsSat) {
        XYZ satPos = mSatPos->get_satPos(sat.satID, obsTime);
        satPositions.push_back(satPos);
    }
    return satPositions;
}

vector<double> SPP::get_Pseudoranges(const obsEpoch& epoch, const vector<XYZ>& satPositions, const XYZ& recPos, SOW sow) {
    vector<double> pseudoranges;
    BLH recPosBLH = mCoordSys->XYZ2BLH(recPos);

    for (size_t i = 0; i < epoch.listObsSat.size(); ++i) {
        const auto& sat = epoch.listObsSat[i];
        const XYZ& satPos = satPositions[i];

        // 假设第一个观测值为伪距
        double pseudorange = sat.values[0];
        Azel azel = mCoordSys->get_azel(recPos, satPos);

        double ionoCorr = mIonoCorr->get_ionoCorr(recPosBLH, azel, sow);
        double tropCorr = mTropCorr->get_tropCorr(recPosBLH, azel.ele);

        pseudorange -= ionoCorr + tropCorr;
        pseudoranges.push_back(pseudorange);
    }
    return pseudoranges;
}

void SPP::leastSquares(const vector<XYZ>& satPositions, const vector<double>& pseudoranges, XYZ& recPos, double& dt) {
    int n = satPositions.size();
    Matrix A(n, 4);
    Matrix L(n, 1);

    for (int i = 0; i < n; ++i) {
        const XYZ& satPos = satPositions[i];
        double rho = sqrt(pow(satPos.x - recPos.x, 2) + pow(satPos.y - recPos.y, 2) + pow(satPos.z - recPos.z, 2));

        A[i][0] = (recPos.x - satPos.x) / rho;
        A[i][1] = (recPos.y - satPos.y) / rho;
        A[i][2] = (recPos.z - satPos.z) / rho;
        A[i][3] = -mGnssConsts->get_SpeedOfLight();

        L[i][0] = pseudoranges[i] - rho - mGnssConsts->get_SpeedOfLight() * dt;
    }

    Matrix N = A.transpose() * A;
    if (!N.invert()) {
        cerr << "Matrix inversion failed." << endl;
        return;
    }

    Matrix X = N * A.transpose() * L;

    recPos.x += X[0][0];
    recPos.y += X[1][0];
    recPos.z += X[2][0];
    dt += X[3][0] / mGnssConsts->get_SpeedOfLight();
}